/*
** --------------------------------------------------------------------------
**
** mshAdapt3D.cpp  -  This header file contains the public  function to obtain
**                    new size of elements in adaptive process.
**
** Author - Antonio Miranda, January - 2008
*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "mshAdapt3D.h"
#include "nfi.h"
#include "auxFunc.h"
#include "shp.h"

#define MAX_ELM_NODES 20 // brick 20
#define MAX_DOF 60       // brick 20 x 3 = 60

/*
** Static functions
** --------------------------------------------------------------------------
*/
static int   NavieStringMatcher( char *T, char *P );
static int   mshAdaptGetNodalResult (TMesh3D *mesh, nfStressTensor *nodalRes);
static void  mshAdaptGetIntPtStress (TMesh3D *mesh, int elemId, nfStressTensor *fem_stress);
static void  mshAdaptNormTensor     (TMesh3D *mesh, int elemId, nfStressTensor *fem_stress, 
                                     double *norm_tensor);
static void  mshAdaptErrorIptTensor (TMesh3D *mesh, int elemId, 
                                     nfStressTensor *nodal_stress, 
			                               nfStressTensor  *diff_stress );
static int   mshAdaptError          (TMesh3D *mesh, nfStressTensor *nodalRes, 
                                     double dErrorPer, double *dErrRatio, 
                                     double *dErrNorm, double *dEnergyNorm);
static int   mshAdaptNewElemSize    (TMesh3D *mesh, double *dErrRatio, double *dErrNorm, 
                                     double *dEnergyNorm, adaptOut *out,
                                     void (*newsize) (double x, double y, 
                                     double z, double new_size));
static void  mshAdaptGaussToNode    (TMesh3D *mesh);




/*
** Export functions
** --------------------------------------------------------------------------
*/

/*
** Msh3DAdapt - main function
** 
** errorPercetage - error percentage used in analysis
** resFile        - file name with results
** out            - error results
** func           - function to return new sizes of elements
*/  
int Msh3DAdapt (double errorPercentage, char *resFile, adaptOut *out, 
                void (*newsize) (double x, double y, double z, double new_size))
{
  FILE *ptr  = NULL;
  fopen_s (&ptr, resFile, "rt");
  int  ok;

  if (ptr == NULL)
    return 0;

  // inicia as classes dos elementos
  shp_init ( );

  // open mesh with results 
  TMesh3D *mesh = new TMesh3D;
  ok = NfiInterp (ptr, mesh);

  fclose (ptr);

  // test status of reading 
  if (!ok)
    return 0;

  // check for gauss results
  if (mesh->results->ElementGauss == NULL)
    return 0;
  else if (mesh->results->ElementGauss->elemInfo == NULL)
    return 0;
  else if (mesh->results->ElementGauss->nfields == 0)
    return 0;

  // check for gauss results
  if (mesh->results->ElementNodal == NULL)
  {
    mshAdaptGaussToNode (mesh);
  }
  else if (mesh->results->ElementNodal->elemInfo == NULL)
    return 0;
  else if (mesh->results->ElementNodal->nfields == 0)
    return 0;

  // create Nodal results based on ElementNodal
  nfStressTensor *nodalRes;
  nodalRes = new nfStressTensor[mesh->nnodes];
  if (!mshAdaptGetNodalResult (mesh, nodalRes))
    return 0;

  // error analysis
  double *dErrRatio, *dErrNorm, *dEnergyNorm;
  dErrRatio   = new double[mesh->nelems];
  dErrNorm    = new double[mesh->nelems]; 
  dEnergyNorm = new double[mesh->nelems];
  if (!mshAdaptError (mesh, nodalRes, errorPercentage, dErrRatio, dErrNorm, dEnergyNorm))
    return 0;


  // compute new size of element and call the feedback functio
  if (!mshAdaptNewElemSize (mesh, dErrRatio, dErrNorm, dEnergyNorm, out, newsize))
    return 0;


#if 0
  FILE *pos = fopen ("v:\\work\\sfem\\exemplos\\error.pos", "wt");
  fprintf(pos, "\n%%RESULT.CASE.STEP.ELEMENT.SCALAR");
  fprintf(pos, "\n%%RESULT.CASE.STEP.ELEMENT.SCALAR.DATA\n");
  int i;
  for (i = 0; i < mesh->nelems; ++i)
    fprintf(pos, "%-4d  %+10.5e  %+10.5e  %+10.5e\n", i+1, dErrRatio[i], dErrNorm[i], dEnergyNorm[i]); 
  fclose (pos);
#endif

  // release memory
  delete []nodalRes;
  delete []dErrRatio;
  delete []dErrNorm; 
  delete []dEnergyNorm;

  return 1;               	
}                	


/*
** Static functions
** --------------------------------------------------------------------------
*/

/* ------------------------------------------------------- */
static int NavieStringMatcher( char *T, char *P )
{
 int n = (int) strlen (T);
 int m = (int) strlen (P);
 int s, k, t;

 for (s = 0; s < n-m+1; s++)
 {
   t = 0;
   for (k = 0; k < m; k++)
   {
     if (P[k] == T[s+k])
       t++;
     else
       k = m;
   }
   if (t == m)
     return 1;
 }
 return 0;
}


/* ========================== mshAdaptGetNodalResult ============================ */

static int  mshAdaptGetNodalResult (TMesh3D *mesh, nfStressTensor *nodalRes)
{
  int  i, j, id;
  int  *nadj_elem;
  ElemResult *currResult = mesh->results->ElementNodal;
  nfElemInfo *curr_info;

  if (currResult == NULL)
    return 0;

  nadj_elem = new int [mesh->nnodes];
  memset (nadj_elem, 0, mesh->nnodes * sizeof (int));
  memset (nodalRes,  0, mesh->nnodes * sizeof (nfStressTensor));

  // obtain the adjacent nodes to nodes
  for (i = 0; i < mesh->nelems; ++i)
  {
    curr_info = &(currResult->elemInfo[i]);
    for (j = 0; j < mesh->elems[i].n; ++j)
    {
      id = mesh->elems[i].inc[j];
      nodalRes[id].sxx += curr_info->values[j][currResult->sxx];
      nodalRes[id].syy += curr_info->values[j][currResult->syy];
      nodalRes[id].szz += curr_info->values[j][currResult->szz];
      nodalRes[id].sxy += curr_info->values[j][currResult->sxy];
      nodalRes[id].syx += curr_info->values[j][currResult->syx];
      nodalRes[id].sxz += curr_info->values[j][currResult->sxz];
      nodalRes[id].szx += curr_info->values[j][currResult->szx];
      nodalRes[id].syz += curr_info->values[j][currResult->syz];
      nodalRes[id].szy += curr_info->values[j][currResult->szy];
      ++nadj_elem[id];
    }
  }

  // node average
  for (i = 0; i < mesh->nnodes; ++i)
  {
    if (nadj_elem[i] == 0)
      continue;

      nodalRes[i].sxx /= nadj_elem[i];
      nodalRes[i].syy /= nadj_elem[i];
      nodalRes[i].szz /= nadj_elem[i];
      nodalRes[i].sxy /= nadj_elem[i];
      nodalRes[i].syx /= nadj_elem[i];
      nodalRes[i].sxz /= nadj_elem[i];
      nodalRes[i].szx /= nadj_elem[i];
      nodalRes[i].syz /= nadj_elem[i];
      nodalRes[i].szy /= nadj_elem[i];
  }

  delete []nadj_elem;

  return 1;
}

// Get integ. point tensor values 
/* ========================== mshAdaptGetIntPtStress ============================ */
static void mshAdaptGetIntPtStress (TMesh3D *mesh, int elemId, nfStressTensor *fem_stress)
{
  ElemResult *currResult = mesh->results->ElementNodal;
  nfElemInfo *curr_info = &(mesh->results->ElementGauss->elemInfo[elemId]);
  int         i;

  memset (fem_stress, 0, sizeof (nfStressTensor));

  for (i = 0; i < curr_info->npts; ++i)
  {
    fem_stress[i].sxx = curr_info->values[i][currResult->sxx];
    fem_stress[i].syy = curr_info->values[i][currResult->syy];
    fem_stress[i].szz = curr_info->values[i][currResult->szz];
    fem_stress[i].sxy = curr_info->values[i][currResult->sxy];
    fem_stress[i].syx = curr_info->values[i][currResult->syx];
    fem_stress[i].sxz = curr_info->values[i][currResult->sxz];
    fem_stress[i].szx = curr_info->values[i][currResult->szx];
    fem_stress[i].syz = curr_info->values[i][currResult->syz];
    fem_stress[i].szy = curr_info->values[i][currResult->szy];
  }
}

// Compute tensor norm for the current element
/* ========================== mshAdaptNormTensor ============================ */
static void mshAdaptNormTensor (TMesh3D *mesh, int elemId, nfStressTensor *fem_stress, 
                                double *norm_tensor)
{
  int          i;               // counters 
  int          num_nodes;       // Number of nodes of a given element 
  int          dim_b_matrix;    // Number of rows of the [B] matrix analisys
  int          num_dof_node;    // Number of degrees of freedom per node 
  int          ndof;            // Number of degrees of freedom 
  double       rigidity_coeff;  // Current element rigidity coefficient 
  double       coeff;           // (rigidity_coeff*det_jac*intpt_wgt[i]) 
  double      *svector;         // AnyTensor[i] in vector form
  double      *svectoraux;      // auxiliar vector 
  double       intpt_wgt;       // Integration points weights 
  double       tprod;           // triple product

  DerivNat   *deriv;
  double      jac[3][3], invjac[3][3], detjac = 0;

  Telement_3d *shp = &(mesh->elems[elemId]);

  // Get number of degrees of freedom 
  num_dof_node = 3;
  num_nodes    = mesh->elems[elemId].n;
  ndof         = num_dof_node * num_nodes;

  // Get number of stress components from Analysis Model Class 
  dim_b_matrix = 6;

  // Get memory for tensor in vector form
  svector     = new double[dim_b_matrix];
  svectoraux  = new double[dim_b_matrix];

  // For each integration point

  i=0;
  *norm_tensor = 0.0;

  // para todos pontos de gauss
  int npg = ShpNumGaussPoint (shp);
  int j;
  for (j = 0; j < npg; j++)
  {
    // derivadas
    ShpDerivMap_i (shp, j, &deriv);
    
    // jacobiano, inversa e determinante
    ShpJacobian (shp, mesh->nodes, deriv, jac, invjac, &detjac);

    // Evaluate the rigidity_coeff according to type of analysis
    rigidity_coeff = 1.0;

    // Put tensor in vector form
    svector[0] = fem_stress[j].sxx;
    svector[1] = fem_stress[j].syy;
    svector[2] = fem_stress[j].szz;
    svector[3] = fem_stress[j].sxy;
    svector[4] = fem_stress[j].sxz;
    svector[5] = fem_stress[j].syz;

    // pesos da integracao numerica
    intpt_wgt = ShpGaussWgt(shp, j);

    // Compute triple product coeff*[s]'[invC][s] from  Math_Lib package
    coeff = rigidity_coeff * detjac * intpt_wgt;

    MathVecMult (6, (double **) mesh->mat[shp->mat-1].invC, svector, svectoraux);

    tprod = MathDotVector(6, svector, svectoraux);

    (*norm_tensor)+= tprod * coeff;

  }

  if (*norm_tensor >= 0.0)
    *norm_tensor = sqrt( *norm_tensor ); 
  else
    *norm_tensor = sqrt( -*norm_tensor ); 

  // Free memory
  delete []svector;
  delete []svectoraux;
}


// ============================== mshAdaptErrorIptTensor =========================== 

static void mshAdaptErrorIptTensor (TMesh3D *mesh, int elemId, 
                                    nfStressTensor *nodal_stress, 
			                              nfStressTensor  *diff_stress )
{
  int            i,j;
  int            node;
  int            num_max_elm_nodes;
  int            num_max_int_pts;
  int            num_map_nodes;
  int            num_shp_nodes;
  int            *connect;
  ShapeVal       *shapefunc = NULL;
  nfStressTensor *elem_nod_stress;   // nodal stress
  nfStressTensor *elem_ipt_stress;   // integration point stress
  nfStressTensor *elem_anl_stress;   // 'analytical' stress

  Telement_3d *shp = &(mesh->elems[elemId]);

  // Get maximum number of integration points 
  num_max_int_pts = ShpNumGaussPoint (shp);

  // Get maximum number of nodes
  num_max_elm_nodes = ShpNumVerts (shp);
  	     
  // Get number of map nodes for the element
  num_map_nodes = ShpNumMapNodes (shp);

  // Get number of shape nodes for the element
  num_shp_nodes = ShpNumShpNodes(shp);

  // Get memory for element stresses and connectivity
  elem_nod_stress = new nfStressTensor[num_max_elm_nodes];
  elem_ipt_stress = new nfStressTensor[num_max_int_pts];
  elem_anl_stress = new nfStressTensor[num_max_int_pts];
  connect = new int[num_max_elm_nodes];

  // Get connectivity for the element
  ShpConnectivity (shp, connect);

  // Update nodal stress for the element based in nodal smoothed stress
  for( i = 0; i < num_map_nodes; i++)
  {
    node = connect[i];
    elem_nod_stress[i].sxx = nodal_stress[node].sxx;
    elem_nod_stress[i].syy = nodal_stress[node].syy;
    elem_nod_stress[i].sxy = nodal_stress[node].sxy;
    elem_nod_stress[i].szz = nodal_stress[node].szz;
    elem_nod_stress[i].sxz = nodal_stress[node].sxz;
    elem_nod_stress[i].syz = nodal_stress[node].syz;

    elem_nod_stress[i].mxx = nodal_stress[node].mxx;
    elem_nod_stress[i].myy = nodal_stress[node].myy;
    elem_nod_stress[i].mxy = nodal_stress[node].mxy;
    elem_nod_stress[i].mzz = nodal_stress[node].mzz;
    elem_nod_stress[i].mxz = nodal_stress[node].mxz;
    elem_nod_stress[i].myz = nodal_stress[node].myz;
  }

  // Compute integration point stress for the element
  mshAdaptGetIntPtStress (mesh, elemId, elem_ipt_stress);

  // Initialize int. point analyt. stress for the element 
  for (i = 0; i < num_max_int_pts; ++i)
    memset (&elem_anl_stress[i], 0, sizeof (nfStressTensor));

  // Compute int. pt. 'analytical' stress for the element, based on smoothed
  // nodal values and element shape functions
  for (i = 0; i < num_max_int_pts; ++i)
  {
    ShpShapeFunc_i (shp, i, &shapefunc);

    for( j = 0; j < num_map_nodes; j++ )
    {
      elem_anl_stress[i].sxx += shapefunc[j] * elem_nod_stress[j].sxx;
      elem_anl_stress[i].syy += shapefunc[j] * elem_nod_stress[j].syy;
      elem_anl_stress[i].sxy += shapefunc[j] * elem_nod_stress[j].sxy;
      elem_anl_stress[i].szz += shapefunc[j] * elem_nod_stress[j].szz;
      elem_anl_stress[i].sxz += shapefunc[j] * elem_nod_stress[j].sxz;
      elem_anl_stress[i].syz += shapefunc[j] * elem_nod_stress[j].syz;

      elem_anl_stress[i].mxx += shapefunc[j] * elem_nod_stress[j].mxx;
      elem_anl_stress[i].myy += shapefunc[j] * elem_nod_stress[j].myy;
      elem_anl_stress[i].mxy += shapefunc[j] * elem_nod_stress[j].mxy;
      elem_anl_stress[i].mzz += shapefunc[j] * elem_nod_stress[j].mzz;
      elem_anl_stress[i].mxz += shapefunc[j] * elem_nod_stress[j].mxz;
      elem_anl_stress[i].myz += shapefunc[j] * elem_nod_stress[j].myz;
    }

  }

  // Get error int. point tensor for the element = ('analytical' - FE solution)
  for (i = 0; i < num_max_int_pts; ++i)
  {
    diff_stress[i].sxx = elem_anl_stress[i].sxx - elem_ipt_stress[i].sxx;
    diff_stress[i].syy = elem_anl_stress[i].syy - elem_ipt_stress[i].syy;
    diff_stress[i].sxy = elem_anl_stress[i].sxy - elem_ipt_stress[i].sxy;
    diff_stress[i].szz = elem_anl_stress[i].szz - elem_ipt_stress[i].szz;
    diff_stress[i].sxz = elem_anl_stress[i].sxz - elem_ipt_stress[i].sxz;
    diff_stress[i].syz = elem_anl_stress[i].syz - elem_ipt_stress[i].syz;

    diff_stress[i].mxx = elem_anl_stress[i].mxx - elem_ipt_stress[i].mxx;
    diff_stress[i].myy = elem_anl_stress[i].myy - elem_ipt_stress[i].myy;
    diff_stress[i].mxy = elem_anl_stress[i].mxy - elem_ipt_stress[i].mxy;
    diff_stress[i].mzz = elem_anl_stress[i].mzz - elem_ipt_stress[i].mzz;
    diff_stress[i].mxz = elem_anl_stress[i].mxz - elem_ipt_stress[i].mxz;
    diff_stress[i].myz = elem_anl_stress[i].myz - elem_ipt_stress[i].myz;
  }

  // Release memory 

  delete []elem_nod_stress;
  delete []elem_ipt_stress;
  delete []elem_anl_stress;

} // End of ErrorIptTensor


/* ========================== mshAdaptError ============================ */

static int mshAdaptError (TMesh3D *mesh, nfStressTensor *nodalRes, double dErrorPer, 
                          double *dErrRatio, double *dErrNorm, double *dEnergyNorm)
{
  int       iNumElem = 0;
  int       num_max_int_pts;
  int       num_mesh_elem;
  double    mesh_error_norm  = 0.0;
  double    mesh_energy_norm = 0.0;
  double    elem_med_error   = 0.0;
  nfStressTensor  *fem_stress;                  // FEM stresses
  nfStressTensor  *diff_stress;                 // (Str(analytical) - Str(FEM))
  int       i;

  // Get maximum number of integration points 
  num_max_int_pts = 27;

  // Get number of mesh elements
  num_mesh_elem = mesh->nelems;

  // Get memory for auxiliar element int. point tensor  
  fem_stress   = new nfStressTensor[num_max_int_pts];
  diff_stress  = new nfStressTensor[num_max_int_pts];

  // Loop in each element to compute element error norm and tensor norm
  for (i = 0; i < num_mesh_elem; ++i)
  {
    // Get integ. point tensor values 
    mshAdaptGetIntPtStress (mesh, i, fem_stress);

    // Compute tensor norm for the current element  ( || u || )
    mshAdaptNormTensor (mesh, i, fem_stress, &dEnergyNorm[i]);

    // Get int. point error tensor 
    mshAdaptErrorIptTensor (mesh, i, nodalRes, diff_stress );

    // Compute error norm for the element ( || e || )
    mshAdaptNormTensor (mesh, i, diff_stress, &dErrNorm[i]);

    // Update mesh norm error and tensor

    mesh_error_norm  += (dErrNorm[i]*dErrNorm[i]);
    mesh_energy_norm += (dEnergyNorm[i]*dEnergyNorm[i]);
  }

  // Compute element medium error. If desired auto-adaptative accuracy 
  // percentage is zero or the mesh  error norm and tensor norm are zero,
  // then the element medium will be zero too 
  if( (dErrorPer==0.0) || (mesh_error_norm==0.0 && mesh_energy_norm==0.0) )
    elem_med_error = 0.0;
  else
    elem_med_error = dErrorPer * sqrt((mesh_error_norm + mesh_energy_norm) / num_mesh_elem);

  // Loop in each element to compute ratio error 
  for (i = 0; i < num_mesh_elem; ++i)
  {
    // Compute ratio error for adaptive process
    if( elem_med_error == 0.0 )
      dErrRatio[i] = 0.0;
    else
      dErrRatio[i] = dErrNorm[i] / elem_med_error;

  }

  // Free memory
  delete []fem_stress;
  delete []diff_stress;

  return 1;
}

#define ABS(a)  (((a)>=0)?(a):(-(a)))

/* ========================== mshAdaptError ============================ */

static int mshAdaptNewElemSize (TMesh3D *mesh, double *dErrRatio, double *dErrNorm, 
                                double *dEnergyNorm, adaptOut *out,
                                void (*newsize) (double x, double y, 
                                double z, double new_size))
{
  double   norm_beb, norm_bub;
  double   new_size, elm_error;
  int i, j, n;
  Tnode_data coord[20];

  // init variables
  out->average_error = 0.0;
  out->max_error     = 0.0;
  out->min_error     = 1.0;
  norm_beb = norm_bub = 0.0;

  // for all elements
  for (i = 0; i < mesh->nelems; ++i)
  {
    Telement_3d *shp = &(mesh->elems[i]);

    // set new element size
    new_size = ShpNewSizeError (shp, mesh->nodes, dErrRatio[i]);
    ShpNodalCoord (shp, mesh->nodes, coord);
    n = ShpNumVerts (shp);
    for (j = 0; j < n; ++j)
      newsize (coord[j].x, coord[j].y, coord[j].z, new_size); 

    // compute errors
    norm_beb += (dErrNorm[i]*dErrNorm[i]);
    norm_bub += (dEnergyNorm[i]*dEnergyNorm[i]);
    elm_error = dErrNorm[i] / sqrt (dErrNorm[i]*dErrNorm[i] + dEnergyNorm[i]*dEnergyNorm[i]);
    if (elm_error < out->min_error)
      out->min_error = elm_error;
    if (elm_error > out->max_error)
      out->max_error = elm_error;
  }
  // average error
  if (ABS(norm_beb + norm_bub) != 0 )
    out->average_error = sqrt( norm_beb/(norm_beb+norm_bub) );

  // get boundbox
  out->min[0] = out->max[0] = mesh->nodes[0].x;
  out->min[1] = out->max[1] = mesh->nodes[0].y;
  out->min[2] = out->max[2] = mesh->nodes[0].z;
  for (i = 1; i < mesh->nnodes; ++i)
  {
    if (mesh->nodes[i].x > out->max[0]) 
      out->max[0] = mesh->nodes[i].x;
    if (mesh->nodes[i].y > out->max[1]) 
      out->max[1] = mesh->nodes[i].y;
    if (mesh->nodes[i].z > out->max[2]) 
      out->max[2] = mesh->nodes[i].z;
    if (mesh->nodes[i].x < out->min[0]) 
      out->min[0] = mesh->nodes[i].x; 
    if (mesh->nodes[i].y < out->min[1]) 
      out->min[1] = mesh->nodes[i].y; 
    if (mesh->nodes[i].z < out->min[2]) 
      out->min[2] = mesh->nodes[i].z;
  }

  return 1;
}


/* ========================== mshAdaptGaussToNode ============================ */

static void mshAdaptGaussToNode (TMesh3D *mesh)
{
  int i;

  // for all elements
  for (i = 0; i < mesh->nelems; ++i)
  {
    Telement_3d *shp = &(mesh->elems[i]);

  }
}